#include<odom_converter/get_odom.h>

using namespace std;

namespace obstacle_avoid
{
    void odomConverter::init(ros::NodeHandle &nh, string &type)
    {
        //读取json文件
        string package_path = ros::package::getPath("odom_converter");
        string file_path = package_path +"/config/test.json";
        ifstream f(file_path);
        if(!f.is_open())
        {
            cout<<"can not find json file! error path"<<endl;
        }
        config_ = json::parse(f);

        //json文件中是否有type
        if(!config_.contains(type))
        {
            cout<<"Invalid type!"<<endl;
        }

        sub_topic = config_[type]["topic"];
        pub_topic = config_[type]["publish"];

        ROS_INFO("Type=%s: Subscribing to [%s], Publishing to [%s]", type.c_str(), sub_topic.c_str(), pub_topic.c_str());

        if(type == "odom")
        {
            topic_sub = nh.subscribe(sub_topic,1,&odomConverter::odomCallback,this);
            topic_pub = nh.advertise<nav_msgs::Odometry>(pub_topic,10);
        }
        else if(type == "pose")
        {
            topic_sub = nh.subscribe(sub_topic,1,&odomConverter::poseCallback,this);
            topic_pub = nh.advertise<geometry_msgs::PoseStamped>(pub_topic,10);
        }
        else
        {
            ROS_ERROR("Unsupported type: %s", type.c_str());
        }
    }

    void odomConverter::odomCallback(const nav_msgs::OdometryConstPtr &msg)
    {
        odom = *msg;
    }

    void odomConverter::poseCallback(const geometry_msgs::PoseStampedConstPtr &msg)
    {
        pose = *msg;
    }

    void odomConverter::run(string &type)
    {
        ros::Rate loop_rate(100);

        while(ros::ok())
        {
            if(type == "odom")
            {
                topic_pub.publish(odom);
            }
            else if(type == "pose")
            {
                topic_pub.publish(pose);
            }
            else
            {
                ROS_ERROR("Unsupported type: %s", type.c_str());
            }

            ros::spinOnce();
            loop_rate.sleep();
        }
    }
}// namespace obstacle_avoid